# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
# 
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.
#
# (C) 2012 Remek Zajac
#!/usr/bin/python

import sets
import math
import threading
import heapq
import gobject

#local modules
import pyPath
import pyOSM2SQL
import pyGeometry
import pyUtils


class Node:
    def __init__(self, waynode, fromNode=None):
        self.waynode = waynode
        self.fromNode = fromNode

    def __hash__(self):
        return self.waynode.id()

    def G(self):
        if self.fromNode:
            return self.distanceFrom(self.fromNode) + self.fromNode.G()
        else:
            return 0

    def distanceFrom(self, fromNode): #in kms
        return pyPath.Path.latlondelta2km(fromNode.latlon(), self.latlon())

    def setFrom(self, fromNode):
        self.fromNode = fromNode

    def H(self, destination):
        #manhattan heuristic
        return self.distanceFrom(destination)

    def F(self, destination):
        return self.G() + self.H(destination)

    def id(self):
        return self.waynode.id()

    def neighbours(self):
        return self.waynode.next()

    def latlon(self):
        return self.waynode.latlon()

    def hasAncestor(self, node):
        if self.fromNode:
            if node.id() == self.fromNode.id():
                return True
            #else:
                #return self.fromNode.hasAncestor()
        return False

    def appendRoute(self, path):
        if self.fromNode:
            self.fromNode.appendRoute(path)
        path.append(pyPath.Path.Milestone(self.latlon(),0))


class ThreadedRouteBuilder(threading.Thread):
    def __init__(self, parent, timer, originWayNode, destinationWayNode):
        super(ThreadedRouteBuilder, self).__init__()
        self.parent = parent
        self.current = None
        self.originWayNode = originWayNode
        self.destinationWayNode = destinationWayNode
        self.timer = timer

    def run(self):      
        dbExtractionTimer = pyUtils.DebugTimeReporter("DB Extraction", self.timer)
        containerSearchesTimer = pyUtils.DebugTimeReporter("Containers", self.timer)

        originNode = Node(self.originWayNode)
        OPEN = {originNode.id(): originNode}
        OPENheapq = [(originNode.F(self.destinationWayNode),originNode)]
        CLOSED = {}
        iterCount = 0
        while OPEN:
            containerSearchesTimer.start()
            self.current = heapq.heappop(OPENheapq)[1]
            #self.current = sorted(OPEN.itervalues(), key=lambda node: node.F(destinationWayNode))[0] <- now that was slow
            containerSearchesTimer.stop()
            
            if self.current.id() == self.destinationWayNode.id():
                gobject.idle_add(self.parent.doneRouting)
                break
            iterCount += 1
            if iterCount%100==0:
                print "routing, iteration:", iterCount
            CLOSED[self.current.id()] = self.current
            del (OPEN[self.current.id()])
            for neighbour in dbExtractionTimer.measure(self.current.neighbours):
                if self.current.hasAncestor(neighbour):
                    continue
                
                if neighbour.id() not in CLOSED:
                    if neighbour.id() not in OPEN:
                        neighbour = Node(neighbour, self.current)
                        OPEN[neighbour.id()] = neighbour
                        heapq.heappush(OPENheapq, (neighbour.F(self.destinationWayNode),neighbour))
                    else:
                        neighbour = OPEN[neighbour.id()]
                else:
                    neighbour = CLOSED[neighbour.id()]
                        
                if neighbour.G() > self.current.G()+neighbour.distanceFrom(self.current):
                    neighbour.setFrom(self.current)


    def dumpRoute(self, container):
        container.clear()
        self.current.appendRoute(container)

    def distance(self):
        distance = self.current.G()
        return distance
     

class Route(pyPath.Path):
    def __init__(self, parent, origin = None, destination = None):
        super(Route, self).__init__(parent)
        self.origin = origin
        self.destination = destination
        self.routeBuilder = None
        self.mainTimer = None

    def setOrigin(self, origin):
        self.origin = origin

    def setDestination(self, destination):
        self.destination = destination

    def do(self, callback):
        self.callback = callback
        self.mainTimer = pyUtils.DebugTimeReporter("Routing")
        self.mainTimer.start()
        nearestNodes = pyUtils.DebugTimeReporter("Finding Origin and Destination", self.mainTimer)
        nearestNodes.start()
        originWayNode = pyOSM2SQL.WayNodes.Instance().getNearest(self.origin)
        destinationWayNode = pyOSM2SQL.WayNodes.Instance().getNearest(self.destination)
        nearestNodes.stop()
        assert(originWayNode and destinationWayNode) #this will need to be more robust
        self.routeBuilder = ThreadedRouteBuilder(self, self.mainTimer, originWayNode, destinationWayNode)
        self.routeBuilder.start()
       
    def doneRouting(self, partial = False):
        self.routeBuilder.join()        
        self.mainTimer.stop()        
        self.routeBuilder.dumpRoute(self)
        self.mainTimer.report()        
        print "Routing complete, route distance:"+('%0.2f'%self.routeBuilder.distance())+'km'
        if self.callback:
            self.callback(self)
        
    def isAlive(self):
        if self.routeBuilder:
            return self.routeBuilder.isAlive()
        return False
    



            


    

    
